/* iBeacon Locator class.
* Neil Z. Shao
*/
#include "json.hpp"
#include "beacon_loc.h"
#include "utils_beacon.h"
#include <fstream>
#include "BeaconInterface.h"

using namespace std;
using namespace easyshow;

#define PI 3.14159265358979
//#define DEBUG_LOG

BeaconLocator::BeaconLocator() {}
BeaconLocator::~BeaconLocator() {}

// setup
void BeaconLocator::addBeaconInfo(BeaconInfo beacon_info)
{
	beacon_infos_.push_back(beacon_info);
	n_beacons_ = beacon_infos_.size();
}

void BeaconLocator::addBeaconMap(BeaconMap beacon_map)
{
	beacon_maps_.push_back(beacon_map);
}

bool BeaconLocator::init(std::string config_fn, bool reset)
{
	// reset
	if (reset) clearAll();

	try {
		// read the file of testing info
		ifstream ifs_info(config_fn);
		if (config_fn.empty() || !ifs_info.is_open()) {
			printf("open config file failed\n");
			return 1;
		}

		nlohmann::json js_node = nlohmann::json::parse(ifs_info);

		// maps
		for (int i = 0; i < js_node["maps"].size(); ++i) {
			nlohmann::json map_node = js_node["maps"][i];

			BeaconMap beacon_map;
			beacon_map.width = map_node["width"];
			beacon_map.height = map_node["height"];
			beacon_map.map_id = map_node.value("map_id", 0);
			beacon_map.map_name = map_node.value("map_name", "map-" + toString(i, 2));
			beacon_map.valid_pos = map_node.value("valid_pos", vector<float>());

			this->addBeaconMap(beacon_map);
		}

		// beacons
		for (int i = 0; i < js_node["beacons"].size(); ++i) {
			nlohmann::json beacon_node = js_node["beacons"][i];

			BeaconInfo beacon_info;
			beacon_info.beaconAddress = beacon_node["beaconAddress"];
			beacon_info.pos_x = beacon_node["pos_x"];
			beacon_info.pos_y = beacon_node["pos_y"];
			beacon_info.map_id = beacon_node["map_id"];
			beacon_info.beacon_id = beacon_node.value("beacon_id", 0);

			if (!beacon_info.beaconAddress.empty())
				this->addBeaconInfo(beacon_info);
		}
	} catch (...) {
		return false;
	}

	return true;
}

// predict
UserPos BeaconLocator::predictPosition(std::vector<BeaconData> beacon_datas)
{
	if (distances_.size() != n_beacons_) {
		distances_.clear();
		distances_.resize(n_beacons_);
	}

	// add received data
	n_records_++;
	for (int i = 0; i < beacon_datas.size(); ++i) {
		BeaconData &rec_beacon = beacon_datas[i];

		int idx = findBeaconIdx(rec_beacon.beaconAddress);
		if (idx >= 0) {
			distances_[idx].push_back(rec_beacon.distance);
		}
	}

	// fill the last data for beacon not seen
	for (int i = 0; i < n_beacons_; ++i) {
		while (distances_[i].size() < n_records_ && distances_[i].size() > 0) {
			distances_[i].push_back(distances_[i].back());
		}
	}

	// remove out-dated data
	for (int i = 0; i < n_beacons_; ++i) {
		while (distances_[i].size() > n_median_) {
			distances_[i].pop_front();
		}
	}

	// find median of received data
	for (int i = 0; i < n_beacons_; ++i) {
#ifdef DEBUG_LOG
		printf("%s %.2f\n", beacon_infos_[i].beaconAddress.c_str(), beacon_infos_[i].distance);
#endif
		beacon_infos_[i].distance = findMedian(distances_[i]);
	}
#ifdef DEBUG_LOG
	printf("----------\n");
#endif

	// predict
	predictPositionParticleFilter(user_pos_);
	
	// smooth
	if (user_pos_.pos_x > 0 && user_pos_.pos_y > 0) {
		const float alpha = 0.5;
		const float beta = 0.25;

		if (last_pos_.pos_x > 0 && last_pos_.pos_y > 0) {
			last_pos_.dir_x = alpha * last_pos_.dir_x * (1 - alpha) * (user_pos_.pos_x - last_pos_.pos_x);
			last_pos_.dir_y = alpha * last_pos_.dir_y * (1 - alpha) * (user_pos_.pos_y - last_pos_.pos_y);

			user_pos_.pos_x = beta * user_pos_.pos_x + (1 - beta) * (last_pos_.pos_x + last_pos_.dir_x);
			user_pos_.pos_y = beta * user_pos_.pos_y + (1 - beta) * (last_pos_.pos_y + last_pos_.dir_y);
		}

		last_pos_ = user_pos_;

		// median filter
		pos_xs_.push_back(user_pos_.pos_x);
		pos_ys_.push_back(user_pos_.pos_y);
		if (pos_xs_.size() > n_median_) pos_xs_.pop_front();
		if (pos_ys_.size() > n_median_) pos_ys_.pop_front();
		user_pos_.pos_x = findMedian(pos_xs_);
		user_pos_.pos_y = findMedian(pos_ys_);

		return user_pos_;
	}

	return last_pos_;
}

UserPos BeaconLocator::predictPosition(std::string beacon_data_str)
{
	nlohmann::json data_node = nlohmann::json::parse(beacon_data_str.c_str());

	vector<BeaconData> datas;
	for (int j = 0; j < data_node["beacons"].size(); ++j) {
		auto &node = data_node["beacons"][j];

		BeaconData dat;
		dat.beaconAddress = node["beaconAddress"];
		dat.distance = node["distance"];

		datas.push_back(dat);
	}

	return predictPosition(datas);
}

#define N_PER_METER 2
void BeaconLocator::predictPositionParticleFilter(UserPos &user_pos)
{
	if (beacon_maps_.empty() || n_records_ < 10)
		return;

	const float map_penalty = 0.2f;

	int n_maps = beacon_maps_.size();
	vector<UserPos> map_pos(n_maps);

	// loop through maps
	for (int k = 0; k < n_maps; ++k) {
		int map_id = beacon_maps_[k].map_id;

		// particles
		BeaconMap &map = beacon_maps_[k];
		vector<UserPos> particles;

		// automatic generate pos
		if (map.valid_pos.size() < 2) {
			const int n_particles = map.width * map.height * N_PER_METER * N_PER_METER;
			particles.resize(n_particles);
			for (int y = 0; y < map.height * N_PER_METER; ++y) {
				for (int x = 0; x < map.width * N_PER_METER; ++x) {
					particles[y * map.width * N_PER_METER + x].pos_x = (float)x / N_PER_METER;
					particles[y * map.width * N_PER_METER + x].pos_y = (float)y / N_PER_METER;
				}
			}
		}

		// load preset pos
		else {
			const int n_particles = map.valid_pos.size() / 2;
			particles.resize(n_particles);
			for (int i = 0; i < n_particles; ++i) {
				particles[i].pos_x = map.valid_pos[2 * i];
				particles[i].pos_y = map.valid_pos[2 * i + 1];
			}
		}

		// evaluate
		float max_conf = 0.f;
		for (int i = 0; i < particles.size(); ++i) {
			if (particles[i].pos_x < 0 || particles[i].pos_y < 0)
				continue;

			particles[i].conf = 0.f;

			// sum of log(likelihood)
			for (int j = 0; j < n_beacons_; ++j) {
				if (beacon_infos_[j].map_id != map_id)
					continue;

				if (beacon_infos_[j].distance <= 0)
					continue;

				// confidence of the measure
				float dist_sigma = beacon_infos_[j].distance * 1;

				// distance to the beacon
				float dist = sqrtf(sqrf(particles[i].pos_x - beacon_infos_[j].pos_x) +
					sqrf(particles[i].pos_y - beacon_infos_[j].pos_y));

				float dist_likelihood = 1.f / sqrtf(2 * PI * sqrf(dist_sigma)) *
					exp(-sqrf(dist - beacon_infos_[j].distance) / (2 * sqrf(dist_sigma)));

				//particles[i].conf += log(dist_likelihood);
				particles[i].conf += dist_likelihood;
			}

			// first or better
			if (i == 0 || particles[i].conf > max_conf) {
				max_conf = particles[i].conf;
				// normalization
				map_pos[k].pos_x = particles[i].pos_x;
				map_pos[k].pos_y = particles[i].pos_y;
				map_pos[k].conf = particles[i].conf;
				map_pos[k].map_id = map_id;
			}
		}
	}

	// choose map
	bool map_changed = false;
	if (user_pos.map_id >= 0 && user_pos.map_id < n_maps) {
		user_pos = map_pos[findMapIdx(user_pos.map_id)];
	} else {
		user_pos = map_pos[0];
		map_changed = true;
	}

	for (int i = 0; i < n_maps; ++i) {
		if ((map_changed && map_pos[i].conf > user_pos.conf) ||
			(!map_changed && map_pos[i].conf > user_pos.conf + map_penalty)) {
			user_pos = map_pos[i];
			map_changed = true;
		}
	}

	// normalization
	user_pos.pos_x /= this->beacon_map(user_pos.map_id).width;
	user_pos.pos_y /= this->beacon_map(user_pos.map_id).height;
}

// clear
void BeaconLocator::clearAll()
{
	clearReceivedData();
	beacon_maps_.clear();
	beacon_infos_.clear();
	n_beacons_ = 0;
	n_records_ = 0;
}

void BeaconLocator::clearReceivedData()
{
	distances_.clear();
	n_records_ = 0;
}

// find index of the received beacon
int BeaconLocator::findBeaconIdx(std::string beaconAddress)
{
	for (int i = 0; i < n_beacons_; ++i) {
		if (beacon_infos_[i].beaconAddress == beaconAddress)
			return i;
	}

	return -1;
}

int BeaconLocator::findMapIdx(int map_id)
{
	for (int i = 0; i < beacon_maps_.size(); ++i)
		if (beacon_maps_[i].map_id == map_id)
			return i;

	return -1;
}
